using System;
using Microsoft.SPOT;

namespace GizmoProject
{
    public class Controller
    {
        private int particlesCount = 200;
        private Robot robot;
        private Board board;
        private ParticlesSet particleSet;

        public Controller()
        {
            this.board = Board.Instance;
            this.robot = new Robot( 2, 3, 90, false, this.board );

            ParticlesSet intialSet = new ParticlesSet( particlesCount, this.board );
            intialSet.InitializeParticles();
            this.particleSet = intialSet;
        }

        public Robot Robot;
        public ParticlesSet Particles;
        Random random = new Random();

        public void DoFiltring()
        {
        /*    if ( Particles.IsDone )
            {
                // MessageBox.Show( "i already know where I am!" );
                return;
            }

            // move the robot
            robot.Move();

            // move the particles
            for ( int i = 0; i < this.particlesCount; i++ )
            {
                this.Particles.Particles[i].SetHeading( robot.Heading );
                //this.Particles.Particles[i].AdvanceBy( robot.speed, robot.noisy );
            }

            ParticlesSet newSet = new ParticlesSet( this.particlesCount, this.board );
            Particle currentParticle;
            List<int> probabilityPoints = new List<int>();

            //constructing helper array of all particles many times for helping choosing ones
            for ( int i = 0; i < this.particlesCount; i++ )
            {
                double particleWeight = this.Particles.Particles[i].weight;
                if ( particleWeight * 1000 < 1 )
                {
                    int t = 0;
                }

                int particleRatio = ( int )( particleWeight * 1000 );

                for ( int j = 0; j < particleRatio; j++ )
                    probabilityPoints.Add( i );
            }

            if ( probabilityPoints.Count == 0 )
                return;

            int particlesNextToOpsticle = 0;
            //fething particles with replasment and counting particlesNextToOpsticle 
            for ( int i = 0; i < this.particlesCount; i++ )
            {
                int index = probabilityPoints[random.Next( 0, probabilityPoints.Count - 1 )];
                currentParticle = this.Particles.Particles[index];
                newSet.Particles[i] = currentParticle.Clone();

                if ( newSet.Particles[i].AdvanceBy( this.robot.speed, this.robot.noisy ) == false )
                {
                    newSet.Particles[i].weight = 0;
                }

                if ( newSet.Particles[i].IsNextToObstacle )
                {
                    particlesNextToOpsticle++;
                }
            }

            if ( particlesNextToOpsticle == 0 )
                particlesNextToOpsticle = 1;

            if ( particlesCount == particlesNextToOpsticle )
                return;


            double weightsSum = 0;

            //calculating new weight of particles 
            if ( robot.IsNextToObstacle )
            {
                for ( int i = 0; i < this.particlesCount; i++ )
                {
                    if ( newSet.Particles[i].weight == 0 )
                    {
                        continue;
                    }
                    if ( newSet.Particles[i].IsNextToObstacle )
                    {
                        newSet.Particles[i].weight = 0.8 / particlesNextToOpsticle;
                    }
                    else
                    {
                        newSet.Particles[i].weight = 0.2 / ( particlesCount - particlesNextToOpsticle );
                    }

                    weightsSum += newSet.Particles[i].weight;
                }
            }
            else
            {
                for ( int i = 0; i < this.particlesCount; i++ )
                {
                    if ( newSet.Particles[i].weight == 0 )
                    {
                        continue;
                    }
                    if ( !newSet.Particles[i].IsNextToObstacle )
                    {
                        newSet.Particles[i].weight = 0.8 / particlesNextToOpsticle;
                    }
                    else
                    {
                        newSet.Particles[i].weight = 0.2 / ( particlesCount - particlesNextToOpsticle );
                    }

                    weightsSum += newSet.Particles[i].weight;
                }
            }

            Debug.Assert( weightsSum != 0 );

            // TODO
            for ( int i = 0; i < this.particlesCount; i++ )
            {
                newSet.Particles[i].weight = newSet.Particles[i].weight / weightsSum;
            }


            # region gaussian kernel
            /*      int err = 0;
                  double sigma = 0.9 * 0.9;
                  double weightsSum = 0;
                  for (int i = 0; i < particlesCount; i++)
                  {
                      int pDist = newSet.Particles[i].getDistToOpsticel();
                  
                      if (robot.IsNextToObstacle)
                      {
                          err =  0 - pDist;
                      }
                      else
                      {
                          err = 5 - pDist;
                      }
                 

                      newSet.Particles[i].weight = Math.Pow(Math.E, -((err * err) / (2 * sigma)));
                      weightsSum += newSet.Particles[i].weight;
                  }
                  for (int i = 0; i < this.particlesCount; i++)
                  {
                      newSet.Particles[i].weight = newSet.Particles[i].weight / weightsSum;
                  }
                  
            # endregion

            this.Particles = newSet;

            // calculate particles weight

            // calculate meanpoint
            this.particleSet.ComputeMeanPoint( this.board );
            */
        }
    }
}
